/**
 * @file    coordinate_transform.h
 * @brief   坐标系转换库(WGS84/ECEF/NED/BODY/CAMERA)
 * @author  yingyq0118@163.com
 * @date    2025-08-30
 * @version V2.00.00
 *
 * @note 所有角度单位均为度，距离单位均为米
 * 大地坐标系LLA(Lon,Lat,Alt) - 经度,纬度,海拔高度
 * 地心地固坐标系ECEF(X,Y,Z) - 原点在地心,Z轴指向北极,X轴指向本初子午线与赤道交点
 * 北东地坐标系NED(N, E, D) - X(N)轴指北,Y(E)轴指东,Z(D)轴指地
 * 机体坐标系Body(X_b, Y_b, Z_b) - 原点在重心,X_b轴机头向前,Y_b轴机翼向右,Z_b轴机腹向下
 * 相机坐标系Camera(X_c, Y_c, Z_c) - 前右下,从相机光心出发,X_c轴向前,Y_c轴向右,Z_c轴向下
 * 所有旋转均遵守右手定则,主动旋转
 * 按照航空标准(321顺序 = ZYX)：
 * Yaw(ψ)：  绕 NED Z 轴(向下)旋转
 * Pitch(θ)：绕 旋转后 Y 轴(横向)旋转
 * Roll(φ)： 绕 机体 X 轴 旋转
 */

#ifndef COORDINATE_TRANSFORM_H
#define COORDINATE_TRANSFORM_H

#ifdef __cplusplus
extern "C"
{
#endif

#include <stdlib.h>
#include <string.h>
#include "eigen.h"

// ==================== 数学常量定义 ====================

/** @brief 圆周率π */
#define M_PI 3.14159265358979323846

/** @brief 度转弧度系数 */
#define DEG2RAD (M_PI / 180.0)

/** @brief 弧度转度系数 */
#define RAD2DEG (180.0 / M_PI)

// ==================== WGS84椭球体参数 ====================

/** @brief a，地球长半轴(赤道半径，米) */
#define EARTH_SEMI_MAJOR 6378137.0

/** @brief b，地球短半轴(极半径，米) */
#define EARTH_SEMI_MINOR 6356752.314245179

/** @brief 地球第一偏心率 e = sqrt(1-(b/a)^2) */
#define EARTH_ECCENTRICITY 0.0818191908426

/** @brief 地球第二偏心率 e = sqrt((a^2-b^2)/b^2 */
#define EARTH_SECOND_ECCENTRICITY 0.0820944379497

    // ==================== 结构体定义 ====================

    /// @brief NED基准坐标系原点参数
    typedef struct
    {
        double base_a0;  // 基准方位角(度)，默认0表示正北方向
        double base_lon; // 基准点经度(度)，范围[-180, 180]
        double base_lat; // 基准点纬度(度)，范围[-90, 90]
        double base_alt; // 基准点高度(米)，默认0
    } Base_LLA;

    /// @brief 相机安装姿态角
    typedef struct
    {
        double roll;  // 安装滚转角(度)
        double pitch; // 安装俯仰角(度)
        double yaw;   // 方位角(度)
    } Camera_Orentation;

    /// @brief 无人机位置姿态信息
    typedef struct
    {
        int id;           // 无人机编号
        double longitude; // 经度(度)->x
        double latitude;  // 纬度(度)->y
        double altitude;  // 高度(米)->z

        double roll;  // 滚转角(度)
        double pitch; // 俯仰角(度)
        double yaw;   // 偏航角(度)
    } Aircraft;

    // ==================== 旋转矩阵计算 ====================

    /**
     * @brief 绕X轴旋转矩阵
     * @param R 输出3x3旋转矩阵 主动旋转
     * @param angle 旋转角度(度)
     */
    void set_rotation_x(Matrix3d *R, double angle_deg);

    /**
     * @brief 绕Y轴旋转矩阵
     * @param R 输出3x3旋转矩阵 主动旋转
     * @param angle 旋转角度(度)
     */
    void set_rotation_y(Matrix3d *R, double angle_deg);

    /**
     * @brief 绕Z轴旋转矩阵
     * @param R 输出3x3旋转矩阵 主动旋转
     * @param angle 旋转角度(度)
     */
    void set_rotation_z(Matrix3d *R, double angle_deg);

    /**
     * @brief ECEF转NED的旋转矩阵
     * @param lng0 基准点经度(度)
     * @param lat0 基准点纬度(度)
     */
    void set_rotation_ecef_to_ned(Matrix3d *R, double lon_deg, double lat_deg);

    /**
     * @brief NED转ECEF的旋转矩阵
     * @param lng0 基准点经度(度)
     * @param lat0 基准点纬度(度)
     */
    void set_rotation_ned_to_ecef(Matrix3d *R, double lon_deg, double lat_deg);

    /**
     * @brief NED到机体系BODY的旋转矩阵
     * @param aircraft 飞机状态数据(包含姿态角)
     * @param[out] rotation_matrix R_ned_to_body = Rz * Ry * Rx
     * @note 先绕Z轴偏航(yaw)，再绕Y轴俯仰(pitch)，最后绕X轴滚转(roll)
     */
    void set_rotation_ned_to_body(const Aircraft *aircraft, Matrix3d *rotation_matrix);

    /**
     * @brief 机体系BODY到NED的旋转矩阵
     * @param aircraft 飞机状态数据(包含姿态角)
     * @param[out] rotation_matrix R_body_to_ned = Rx(-) * Ry(-) * Rz(-)
     * @note ZYX内旋
     */
    void set_rotation_body_to_ned(const Aircraft *aircraft, Matrix3d *rotation_matrix);

    /**
     * @brief 机体系BODY到相机系CAMERA(RDF)的旋转矩阵
     * @param camera_orenation 相机安装参数
     * @param[out] rotation_matrix = Rz * Ry * Rx
     * @note 考虑相机安装偏移和角度偏差
     */
    void set_rotation_body_to_camera(const Camera_Orentation *camera_orenation, Matrix3d *rotation_matrix);

    /**
     * @brief 相机系CAMERA(RDF)到机体系BODY的旋转矩阵
     * @param camera_orenation 相机安装参数
     * @param[out] rotation_matrix = Rx(-) * Ry(-) * Rz(-)
     * @note 考虑相机安装角度偏差
     */
    void set_rotation_camera_to_body(const Camera_Orentation *camera_orenation, Matrix3d *rotation_matrix);

    /**
     * @brief 经纬高(LLA)转地心地固坐标系(ECEF)
     * @param lla LLA坐标[经度,纬度,高度](度,度,米)
     * @param[out] ecef 输出的ECEF坐标[X,Y,Z](米)
     */
    void lla_to_ecef(const Vector3d *lla, Vector3d *ecef);

    /**
     * @brief 地心地固坐标系(ECEF)转经纬高(LLA)
     * @param ecef ECEF坐标[X,Y,Z](米)
     * @param[out] lla 输出的LLA坐标[经度,纬度,高度](度,度,米)
     */
    void ecef_to_lla(const Vector3d *ecef, Vector3d *lla);

    /**
     * @brief 地心地固系(ECEF)转北东地坐标系(NED)
     * @param base_lla 基准点LLA参数
     * @param ecef ECEF坐标[X,Y,Z](米)
     * @param[out] ned 输出的NED坐标[北,东,地](米)
     */
    void ecef_to_ned(const Base_LLA *base_lla, const Vector3d *ecef, Vector3d *ned);

    /**
     * @brief 北东地坐标系(NED)转地心地固系(ECEF)
     * @param ned NED坐标[北,东,地](米)
     * @param base_lla 基准点LLA参数
     * @param[out] ecef 输出的ECEF坐标[X,Y,Z](米)
     */
    void ned_to_ecef(const Base_LLA *base_lla, const Vector3d *ned, Vector3d *ecef);

    /**
     * @brief 北东地(NED)转机体坐标系(BODY)
     * @param ned 输入NED坐标 [北, 东, 地](米)
     * @param aircraft 飞机状态(包含姿态角)
     * @param[out] body 输出BODY坐标 [前, 右, 下](米)
     * @note 使用 Z-Y-X 欧拉角计算旋转矩阵
     */
    void ned_to_body(const Vector3d *ned, const Aircraft *aircraft, Vector3d *body);

    /**
     * @brief 机体坐标系(BODY)转北东地(NED)
     * @param body 输入BODY坐标 [前, 右, 下](米)
     * @param aircraft 飞机状态(包含姿态角)
     * @param[out] ned 输出NED坐标 [北, 东, 地](米)
     * @note 使用 Z-Y-X 欧拉角计算旋转矩阵
     */
    void body_to_ned(const Vector3d *body, const Aircraft *aircraft, Vector3d *ned);

    /**
     * @brief 机体坐标系(BODY)转相机坐标系(CAMERA)
     * @param body 输入BODY坐标 [前, 右, 下](米)
     * @param camera_orenation 相机安装参数[姿态角](度)
     * @param[out] camera 输出CAMERA坐标
     * @note 考虑安装偏角，采用右手旋转矩阵
     */
    void body_to_camera(const Vector3d *body, const Camera_Orentation *camera_orenation, Vector3d *camera);

    /**
     * @brief 相机坐标系(CAMERA)转机体坐标系(BODY)
     * @param camera 输入CAMERA坐标
     * @param camera_orenation 相机安装参数
     * @param[out] body 输出BODY坐标(米)
     * @note 考虑安装偏角，采用右手旋转矩阵
     */
    void camera_to_body(const Vector3d *camera, const Camera_Orentation *camera_orenation, Vector3d *body);

    /**
     * @brief 经纬高(LLA)直接转北东地(NED)
     * @param base_lla 基准点参数
     * @param lla 输入的LLA坐标[经度,纬度,高度](度,度,米)
     * @param[out] ned 输出的NED坐标[北,东,地](米)
     * @note 内部调用lla_to_ecef+ecef_to_ned实现
     */
    void lla_to_ned(const Base_LLA *base_lla, const Vector3d *lla, Vector3d *ned);

    /**
     * @brief 北东地(NED)直接转经纬高(LLA)
     * @param base_lla 基准点参数
     * @param ned 输入的NED坐标[北,东,地](米)
     * @param[out] lla 输出的LLA坐标[经度,纬度,高度](度,度,米)
     * @note 内部调用ned_to_ecef+ecef_to_lla实现
     */
    void ned_to_lla(const Base_LLA *base_lla, const Vector3d *ned, Vector3d *lla);

    /**
     * @brief 计算两点之间的三维直线距离(包含高度差)
     * @param base_lla 基准点LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla1 点1的LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla2 点2的LLA坐标[经度,纬度,高度](度,度,米)
     * @return distance 距离(米) = lla2 - lla1 = √(ΔN² + ΔE² + ΔD²)
     * @note 内部调用lla_to_ned实现
     */
    double cal_3d_distance(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2);

    /**
     * @brief 计算两点之间的水平地面距离(忽略高度差)
     * @param base_lla 基准点LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla1 点1的LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla2 点2的LLA坐标[经度,纬度,高度](度,度,米)
     * @return 水平地面距离(米) = lla2 - lla1 = √(ΔN² + ΔE²)
     * @note 内部调用lla_to_ned实现
     */
    double cal_horizontal_distance(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2);

    /**
     * @brief 计算两点之间的XY平面方位角(真北顺时针)
     * @param base_lla 基准点LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla1 起点的LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla2 终点的LLA坐标[经度,纬度,高度](度,度,米)
     * @return bearing 方位角(0-360度)从正北开始顺时针的角度
     * @note 内部调用lla_to_ned实现
     */
    double cal_bearing(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2);

    /**
     * @brief 计算从点1到点2的俯仰角
     * @param base_lla 基准点LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla1 起点的LLA坐标[经度,纬度,高度](度,度,米)
     * @param lla2 终点的LLA坐标[经度,纬度,高度](度,度,米)
     * @return elevation 俯仰角(度,向上为正)
     * @note 内部调用lla_to_ned实现
     */
    double cal_elevation(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2);

#ifdef __cplusplus
}
#endif

#endif // COORDINATE_TRANSFORM_H